#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Point
import cv2
import numpy as np
import time

class FaceErrorPublisher(Node):
    def __init__(self):
        super().__init__('face_error_publisher')
        # 发布到 /result3_topic
        self.pub = self.create_publisher(Point, '/result3_topic', 10)

        # 打开摄像头索引（根据 v4l2-ctl 确认）
        idx = 4
        self.cap = cv2.VideoCapture(idx)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        if not self.cap.isOpened():
            self.get_logger().error(f"无法打开摄像头 /dev/video{idx}")
            raise RuntimeError("Camera open failed")

        # 加载人脸检测模型
        self.face_cascade = cv2.CascadeClassifier(
            cv2.data.haarcascades + "haarcascade_frontalface_default.xml"
        )
        if self.face_cascade.empty():
            self.get_logger().error("无法加载人脸检测模型")
            raise RuntimeError("Cascade load failed")

        cv2.namedWindow("Face Error Preview", cv2.WINDOW_NORMAL)
        self.get_logger().info("节点已启动，按 'q' 键退出")

        self.prev_time = time.time()
        # 每 0.03s 调用一次处理循环（约 30Hz）
        self.timer = self.create_timer(0.03, self.timer_callback)

        # 平滑滤波参数
        self.smooth_N = 5  # 滑动窗口大小
        self.e_x_buf = []
        self.e_y_buf = []

    def timer_callback(self):
        ret, frame = self.cap.read()
        if not ret:
            self.get_logger().warn("获取帧失败")
            return

        # 计算和显示帧率
        curr_time = time.time()
        fps = 1.0 / (curr_time - self.prev_time) if curr_time != self.prev_time else 0.0
        self.prev_time = curr_time

        h, w = frame.shape[:2]
        frame_cx, frame_cy = w//2, h//2

        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        faces = self.face_cascade.detectMultiScale(
            gray, scaleFactor=1.1, minNeighbors=5, minSize=(60,60)
        )

        # 默认误差点为画面中心
        e_x, e_y = 0.0, 0.0

        if len(faces) > 0:
            # 选最大人脸
            x, y, fw, fh = max(faces, key=lambda r: r[2]*r[3])
            face_cx = x + fw//2
            face_cy = y + fh//2

            # 计算像素误差
            e_x = float(face_cx - frame_cx)
            e_y = float(face_cy - frame_cy)

            # 可视化人脸框与中心
            cv2.rectangle(frame, (x,y), (x+fw, y+fh), (255,0,0), 2)
            cv2.circle(frame, (face_cx, face_cy), 5, (0,255,0), -1)

        # === 平滑滤波 ===
        self.e_x_buf.append(e_x)
        self.e_y_buf.append(e_y)
        if len(self.e_x_buf) > self.smooth_N:
            self.e_x_buf.pop(0)
            self.e_y_buf.pop(0)
        smooth_e_x = float(np.mean(self.e_x_buf))
        smooth_e_y = float(np.mean(self.e_y_buf))

        # === 最大值限制 ===
        smooth_e_x = max(min(smooth_e_x, 100), -100)
        smooth_e_y = max(min(smooth_e_y, 100), -100)

        # 明确转换为 Python float
        pt = Point()
        pt.x = -float(smooth_e_x)
        pt.y = float(smooth_e_y)
        pt.z = 0.0  # 深度暂时设为 0
        self.pub.publish(pt)

        # 在图上显示误差与 FPS
        cv2.putText(frame, f"Error: ({smooth_e_x:.1f}, {smooth_e_y:.1f})",
                    (10, h-30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,255), 2)
        cv2.putText(frame, f"FPS: {fps:.1f}",
                    (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2)

        cv2.imshow("Face Error Preview", frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            # 按 q 退出
            rclpy.shutdown()

    def destroy_node(self):
        self.cap.release()
        cv2.destroyAllWindows()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = FaceErrorPublisher()
    rclpy.spin(node)
    node.destroy_node()

if __name__ == '__main__':
    main()
